
        INCLUDE kxarm.h
        INCLUDE armmacros.s

        IMPORT  UndefHandler
        IMPORT  SWIHandler
        IMPORT  PrefetchAbortHandler
        IMPORT  DataAbortHandler
        IMPORT  IRQHandler
        IMPORT  FIQHandler

        EXPORT  VectorTable

        ;-----------------------------------------------------------------------
        ; NOTE: VectorTable MUST BE IN Code (Read-only TEXT section)
        TEXTAREA

        NESTED_ENTRY UndefException
        sub     lr, lr, #4                      ; fix return address
        stmfd   sp!, {r0-r3, r12, lr}
        PROLOG_END
        CALL    UndefHandler
        ldmfd   sp!, {r0-r3, r12, pc}^          ; restore regs & return for NOP
        ENTRY_END UndefException
        LTORG


; it seems that can't call swi in svc mode, why!!!
        NESTED_ENTRY SWIException
        stmfd   sp!, {r0-r3, r12, lr}
        mov r1, sp
        ;save spsr for maybe call in svc mode
        mrs r0, spsr
        stmfd sp!, {r0}
        PROLOG_END
        ;get swi code
        ldr r0, [lr, #-4]
        bic r0, r0, #0xff000000
    mov r2, lr
    mrs r3, spsr
       CALL    SWIHandler
        ;restore spsr for maybe call in svc mode
        ldmfd   sp!, {r0}
        msr spsr_cxsf, r0
        ; return
        ldmfd   sp!, {r0-r3, r12, pc}^          ; restore regs & return for NOP
        ENTRY_END SWIException
        LTORG


        NESTED_ENTRY PrefetchAbortException
        sub     lr, lr, #4                    	 ; fix return address
        stmfd   sp!, {r0-r3, r12, lr}
        PROLOG_END
        mov r0, lr
        CALL    PrefetchAbortHandler
        ldmfd   sp!, {r0-r3, r12, pc}^          ; restore regs & return for NOP
        ENTRY_END PrefetchAbortException
        LTORG


        NESTED_ENTRY DataAbortException
        sub     lr, lr, #8                    	 ; fix return address
        stmfd   sp!, {r0-r3, r12, lr}
        PROLOG_END

        mrc	p15, 0, r0, c6, c0, 0 	 ; (r0) = FAR
        mrc	p15, 0, r1, c5, c0, 0		; (r1) = FSR
	mrs	r2, spsr                     	; (r1) = saved status reg
        mov r3, lr
        CALL    DataAbortHandler
        ldmfd   sp!, {r0-r3, r12, pc}^          ; restore regs & return for NOP
        ENTRY_END DataAbortException
        LTORG


        NESTED_ENTRY IRQException
        sub     lr, lr, #4                      ; fix return address
        stmfd   sp!, {r0-r3, r12, lr}
        PROLOG_END
        CALL    IRQHandler
        ldmfd   sp!, {r0-r3, r12, pc}^          ; restore regs & return for NOP
        ENTRY_END IRQException
        LTORG


        NESTED_ENTRY FIQException
        sub     lr, lr, #4                      ; fix return address
        stmfd   sp!, {r0-r3, r12, lr}
        PROLOG_END
        CALL    FIQHandler
        ldmfd   sp!, {r0-r3, r12, pc}^          ; restore regs & return for NOP
        ENTRY_END FIQException
        LTORG



        LEAF_ENTRY FillVectors
        ldr r1, =VectorTable
        mov r2, #8

10
	ldr r3, [r1], #4
	str r3, [r0], #4
        sub     r2, r2, #1
        cmp     r2, #0
        bne     %B10 

	mov     pc, lr                          ; return
       	ENTRY_END


VectorTable
        DCD     -1                              		; reset
        DCD     UndefException                  	; undefined instruction
        DCD     SWIException                     ; SVC
        DCD     PrefetchAbortException     	; Prefetch abort
        DCD     DataAbortException         	; data abort
        DCD     -1                              		; unused vector
        DCD     IRQException                      ; IRQ
        DCD     FIQException                      ; FIQ



        LEAF_ENTRY SVC_Call_ArgNumb_0
        LEAF_ENTRY SVC_Call_ArgNumb_1
        LEAF_ENTRY SVC_Call_ArgNumb_2
        LEAF_ENTRY SVC_Call_ArgNumb_3
        swi 0x40
	mov     pc, lr                          ; return
       	ENTRY_END
        

        END


